% Running the reinforcement learning test. Starting with agent in map at given (pos)ition

function collectedReward = hw7Run(agent, map, pos)
%Setting variables and obtaining map information
gMap = map(:,:,1);
obstacleMap = map(:,:,2);
litterMap = map(:,:,3);
[RMAX,CMAX,mWorlds] = size(map);
agentPosx = pos(1);
agentPosy = pos(2);
RMIN = 1;
CMIN  = 1;
distFromEnd = 0;
distFromMidPath = 0;
collectedReward = 0;
grassQtable = agent.grass;
litterQtable = agent.litter;
% Number of actions in every state. 5 - look E,N,N,NE,SE
%Let 1-N, 2-NE, 3-E, 4-SE, 5-S
NUM_ACTIONS = 5;
% Iterate over the number of episodes

%Find a path in the map
i=1;
pathStart = 0;
pathSize = 0;
igMap = gMap;
while (i<=RMAX)
    if(gMap(i,1) == -1)
        i = i+1;

        continue
    else
        % found path
        pathStart = i;
        while(i<=RMAX && gMap(i,1) == 0)
            igMap(i,:) = 255;
            pathSize = pathSize + 1;
            i = i+1;
        end
    end
end

distFromEnd = CMAX - agentPosy;	
distFromMidPath = agentPosx - uint8((pathStart+pathSize/2));
findNewVal = 0;
image(igMap);
hold on
pos1 = [agentPosx,agentPosy];
pos2 = [agentPosx,agentPosy];
newposi = zeros(1,2);
while (distFromEnd > 0)
    distFromEnd;
    if (findNewVal)
        nextDir = uint8((NUM_ACTIONS-1)*rand(1)+1);
        findNewVal = 0;
    else
        lookupdistFromMidPath = distFromMidPath;
        if(distFromMidPath <=0)
            lookupdistFromMidPath = distFromMidPath + 25;
        end
        lookupdistFromEnd = distFromEnd+1;
        [maxVal,nextDir] = max(grassQtable(lookupdistFromMidPath,lookupdistFromEnd,:));
    end

   	if(nextDir == 1) % North
			if(agentPosx == RMIN)
                findNewVal = 1;
				continue
			end
			agentPosx = agentPosx - 1;		
    elseif(nextDir == 2) % North East
        if(agentPosx == RMIN)
                findNewVal = 1;
				continue

        end
        agentPosx = agentPosx - 1;		
        agentPosy = agentPosy + 1;		
    elseif(nextDir == 3) % East
        if(agentPosy == CMAX)
                findNewVal = 1;
				continue

        end
        agentPosy = agentPosy + 1;		
    elseif(nextDir == 4) % South East
        if(agentPosx == RMAX)
                findNewVal = 1;
				continue

        end
        agentPosx = agentPosx + 1;		
        agentPosy = agentPosy + 1;		
    elseif(nextDir == 5) % South
        if(agentPosx == RMAX)
                findNewVal = 1;
				continue

        end
        agentPosx = agentPosx + 1;		
    end
    h = plot(agentPosy,agentPosx,'.');
    newposi = [agentPosx,agentPosy];
    if(newposi == pos2)
        findNewVal = 1;
        continue
    else
        pos2 = pos1;
        pos1 = newposi;
    end
    set(h,'MarkerSize',24)
    hold on
    %correction based on new information
    reward = gMap(agentPosx,agentPosy);
    collectedReward = collectedReward + reward;
    distFromEnd = CMAX - agentPosy;	
    distFromMidPath = agentPosx - uint8((pathStart+pathSize/2));
end

